/**
 * @file roadblock.c
 * @author sevenfite (sevenfite@163.com)
 * @brief 路障方法
 * @version 0.1
 * @date 2024-01-09
 *
 * @copyright Copyright (c) 2024
 *
 * @note 通过改变巡线方法和特定动作来实现路障方法
 */
#include "roadblock.h"

#include "C_code_Log.h"
#include "control.h"
#include "findline.h"
#include "motor.h"
#include "pid.h"
#include "camera.h"
#include "gyro.h"
#include "Mydelay.h"
#include "distance.h"
#include "photoelectric.h"
#include "stdlib.h"
#include "jq8400.h"
#include "voice.h"
#include "servo.h"

//将过障碍检测的角度进行封装
#define CAR_ATTITUDE_flat (inRange((int)gyro_getPitch(),-4,4)) //检测到过完障碍，在平地上
#define CAR_ATTITUDE_Step_up (gyro_getPitch()<-6)    //检测上台阶
#define CAR_ATTITUDE_Step_UP_ING (gyro_getPitch()<-12) //检测上台阶中
#define CAR_ATTITUDE_Step_down (gyro_getPitch()>10)  //检测下台阶
#define CAR_ATTITUDE_Wave_up (gyro_getPitch()<-5)    //检测上波浪板
#define CAR_ATTITUDE_Wave_down (gyro_getPitch()>11)  //检测下波浪板
#define CAR_ATTITUDE_Wave_stabdy (inRange(gyro_getPitch(),-5,5) && inRange(gyro_getRoll(),-3,3))   //开始平稳行驶了
#define CAR_ATTITUDE_Wave_Nostabdy (!(CAR_ATTITUDE_Wave_stabdy)) //又不平稳了
#define CAR_ATTITUDE_Bridge_up (inRange((int)gyro_getPitch(),-8,-20)) //检测上桥
#define CAR_ATTITUDE_Bridge_down (inRange((int)gyro_getPitch(),8,12)) //检测下桥
#define CAR_ATTITUDE_Platform_down (gyro_getPitch()>4) //检测下平台
#define CAR_ATTITUDE_Platform_down_finish (gyro_getPitch()<8) //检测下完平台
#define CAR_ATTITUDE_Platform_down_ing (inRange((int)gyro_getPitch(),16,22)) //检测下平台中
#define CAR_ATTITUDE_Hill_Peak_up (gyro_getPitch()<-8) //小山丘平台和珠峰平台，检测上坡
#define CAR_ATTITUDE_Hill_Peak_up_end (gyro_getPitch()>-5) //小山丘平台和珠峰平台，检测上完坡
#define CAR_ATTITUDE_HillDown_PeakDown_down (gyro_getPitch()>7) //小山丘平台和珠峰平台，检测下坡
#define CAR_ATTITUDE_PeakDown_down_end (gyro_getPitch()<5) //珠峰平台，检测下完平台
   
/*

* 函数介绍：过台阶方法
* 输入参数：无
* 输出参数：无
* 返回值  ：1(路障解决)0（路障未解决）
* 其他		：无
* 作者    ：@袁梓聪

*/
static uint8_t BlockHandleMethod_Step() {
  static enum Flag{
    INIT,//初始化
    FLAT,//平地
    PRE_UP,//在台阶前准备上台阶
    STOP,//停止
    STEP_UP,//上台阶
    STEP_ON,//在台阶上
    STEP_DOWN//下台阶
  }flag;
  static FindLine save;
  if (flag == INIT) {
    LOG_INFO("台阶障碍方法");
    save = gl_controlCenter.findLineWay;
    flag = FLAT;
  }
  if(flag == FLAT && CAR_ATTITUDE_Step_up){//检测刚碰到台阶
    LOG_INFO("检测到台阶");
    gl_controlCenter.findLineWay = FL_Step;//设置巡线方法为上台阶
    gl_controlCenter.findLineFlag = 0;
    //切换为"上台阶"状态,二轮驱动版本则应切换为"在台阶前准备上台阶"(PRE_UP)状态,避免冲太快导致后翻
    flag = STEP_UP;
  }
  if(flag == PRE_UP && CAR_ATTITUDE_Step_UP_ING){
    LOG_INFO("车已大部分在台阶上,刹停一段时间");
    motorStop();
    flag = STOP;
  }
  if(flag == STOP && myDelay((uint32_t)BlockHandleMethod_Step,200)){
    LOG_INFO("刹停结束,继续上台阶");
    gl_controlCenter.findLineWay = FL_Step;
    gl_controlCenter.findLineFlag = 0;
    flag = STEP_UP;
  }
  if(flag == STEP_UP){
    flag = STEP_ON;
  }
  if(flag == STEP_ON && CAR_ATTITUDE_Step_down){//检测下台阶
    LOG_INFO("检测到下台阶");
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    flag = INIT;
    return 1;
  }
  return 0;
}
/*

* 函数介绍：过波浪板方法
* 输入参数：无
* 输出参数：无
* 返回值  ：1(路障解决)0（路障未解决）
* 其他		：无
* 作者    ：@袁梓聪

*/
static uint8_t BlockHandleMethod_Wave() {
  static enum Flag{
    INIT,//初始化
    ON,//上波浪板
    DOWN,//下波浪板
    STABLE//平稳
  }flag;
  static FindLine save;
  static uint32_t stableTime=0;//首次稳定的全局时间
  if(flag == INIT && CAR_ATTITUDE_Wave_up){//检测上波浪板
    save = gl_controlCenter.findLineWay;
    gl_controlCenter.findLineWay = FL_WAVE;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = ON;
  }
  //检测下波浪板
  if(flag == ON && CAR_ATTITUDE_Wave_down)
  {
    flag = DOWN;
  }
  //开始平稳行驶了
  if(flag == DOWN && CAR_ATTITUDE_Wave_stabdy){
    stableTime = getGlobalTime();
    flag = STABLE;
  }
  //又不平稳了
  if(flag == STABLE && CAR_ATTITUDE_Wave_Nostabdy){
    flag = DOWN;
  }
  //平稳时间超过0.1秒
  if(flag == STABLE && getGlobalTime()-stableTime>=200){
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = INIT;
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  //没检测到出波浪板
  if(flag == ON && CAR_ATTITUDE_Wave_Nostabdy){
    stableTime = getGlobalTime();
  }
  if(flag == ON && CAR_ATTITUDE_Wave_stabdy && getGlobalTime()-stableTime>800){
    flag = INIT;
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  return 0;
}
/**
 * @brief 过波浪板方法，通过时间来控制
 * @param time 通过的时间
 * @note 通过定时的方式来决定是否已经过波浪板
 * @return uint8_t 0：未过波浪板 1：已经过波浪板
 */
static uint8_t BlockHandleMethod_Wave_time(uint16_t time) {
  static enum Flag{
    INIT,//初始化
    ON,//上波浪板
  }flag;
  static FindLine save;
  if(flag == INIT && CAR_ATTITUDE_Wave_up){//检测上波浪板
    LOG_INFO("波浪板障碍方法");
    save = gl_controlCenter.findLineWay;
    gl_controlCenter.findLineWay = FL_WAVE;
    gl_controlCenter.findLineFlag = 0;
    flag = ON;
  }
  if(flag == ON && myDelay((uint32_t)(BlockHandleMethod_Wave_time),time)){
    LOG_INFO("时间过了%dms,认为已经出波浪板",time);
    flag = INIT;
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  return 0;
}
/**
 * @brief 过桥方法
 * 
 * @return uint8_t 
 */
static uint8_t BlockHandleMethod_Bridge() {
  //桥的长度
  #define BRIDGE_DISTANCE 800 
  #define BRIDGE_UP_ANGLE_ARR_LEN 5 //上桥时校正用的角度数组长度
  static enum {
    BRIDGE_INIT,//初始化
    BRIDGE_FIND_LINE_GROUND,//刚开始执行函数的时候，在地面上巡线
    BRIDGE_GET_ANGLE,//获取平均角度，用于后面的盲走
    BRIDGE_GYRO_GROUND,//陀螺仪盲走，在地面上
    BRIDGE_UP,//上桥过程
    BRIDGE_ON,//上桥后，在桥上
    BRIDGE_ON_END,//桥上的末端
    BRIDGE_DOWN//下桥过程
  } ENUM_Flag = BRIDGE_INIT;
  static struct RoadblockBridge
  {
    FindLine save;
    int16_t startSpeed;
    uint32_t distanceTarget;
    float angleArr[BRIDGE_UP_ANGLE_ARR_LEN];
    uint8_t angleIndex; 
    float angleAverage;
    uint32_t lastTime;
  }*static_variable =NULL;
  if(ENUM_Flag == BRIDGE_INIT){
    //初始化变量
    static_variable = (struct RoadblockBridge *)malloc(sizeof(struct RoadblockBridge));
    static_variable->angleIndex = 0;
    if(static_variable == NULL){//内存分配失败
      motorStop();
      LOG_ERROR("Bridge variable member malloc failed");
      while(1);
    }
    //正常巡线
    ENUM_Flag=BRIDGE_FIND_LINE_GROUND;
  }
  if (ENUM_Flag == BRIDGE_FIND_LINE_GROUND &&
      MyDistance((uint32_t)BlockHandleMethod_Bridge, 30)) {//在地面上巡线30ms后，开始获取角度
      //记录当前的巡线模式
    static_variable->save = gl_controlCenter.findLineWay;
    //记录当前的速度
    static_variable->startSpeed = (getLeftMotorPWM()+getRightMotorPWM())/2;
    ENUM_Flag = BRIDGE_GET_ANGLE;
    static_variable->lastTime = getGlobalTime();
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
  }
  if(ENUM_Flag == BRIDGE_GET_ANGLE){
    //获取10个角度的平均值
    if(getGlobalTime()-static_variable->lastTime<50){//50ms获取一次,避免频繁获取
      return 0;
    }
    else if(static_variable->angleIndex<BRIDGE_UP_ANGLE_ARR_LEN){
      static_variable->angleArr[static_variable->angleIndex] = gyro_getYaw();
      static_variable->angleIndex++;
      static_variable->lastTime = getGlobalTime();
    }else{
      //去除一个最大值，一个最小值，求平均值，开始盲走
      static_variable->angleAverage = yawAngleAverage(static_variable->angleArr,BRIDGE_UP_ANGLE_ARR_LEN,1);
      gl_controlCenter.findLineWay = FL_No;
      ENUM_Flag = BRIDGE_GYRO_GROUND;
      HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    }
  }
  if(ENUM_Flag == BRIDGE_GYRO_GROUND){
    //陀螺仪盲走，速度为刚开始的速度
   alongGyro(static_variable->angleAverage, static_variable->startSpeed);
  }
  if(ENUM_Flag == BRIDGE_GYRO_GROUND && CAR_ATTITUDE_Bridge_up){//检测上桥
  //在上桥过程中是按陀螺仪盲走的
   gl_controlCenter.findLineWay = NFL_GYRO_UP;
   gl_controlCenter.findLineFlag = 0;
    ENUM_Flag = BRIDGE_UP;
  }
  if(ENUM_Flag == BRIDGE_UP && CAR_ATTITUDE_flat){//检测到桥上
    camera_SendCommand(CAMERA_CMD_BRIDGE);
    gl_controlCenter.findLineWay = FL_bridge;
    gl_controlCenter.findLineFlag = 0;
    //根据此时的编码器值和桥的长度，算出桥末端的编码器值
    static_variable->distanceTarget = gl_motor.encoderDistance+ BRIDGE_DISTANCE * ENCODE_OF_ONE_mm;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    ENUM_Flag = BRIDGE_ON;
  }
  //根据编码器知道到了桥的末端，将速度改小
  if(ENUM_Flag == BRIDGE_ON && gl_motor.encoderDistance > static_variable->distanceTarget)
  {
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    gl_controlCenter.findLineWay = FL_bridge_slow;
    gl_controlCenter.findLineFlag = 0;
    ENUM_Flag = BRIDGE_ON_END;
  }
  //根据陀螺仪知道已经下桥了，把寻线模式切为正常，速度为慢速
  if(ENUM_Flag == BRIDGE_ON_END && CAR_ATTITUDE_Bridge_down){//检测下桥
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    camera_SendCommand(CAMERA_CMD_FIND_LINE);
    //下桥时慢速
    gl_controlCenter.findLineWay = FL_DOWN_LONG;//下桥的坡度比较大,不能用下平台的
    gl_controlCenter.findLineFlag = 0;
    ENUM_Flag = BRIDGE_DOWN;
  }
  if(ENUM_Flag == BRIDGE_DOWN && CAR_ATTITUDE_flat){//检测下完桥
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    gl_controlCenter.findLineWay = static_variable->save;
    gl_controlCenter.findLineFlag = 0;
    ENUM_Flag = BRIDGE_INIT;
    free(static_variable);
    return 1;
  }
  return 0;
}
/**
 * @brief 过刀山的处理方法
 * 
 * @return uint8_t 
 */
static uint8_t BlockHandleMethod_Knife() {
  static FindLine save;
  static uint8_t flag = 0;
  if(flag == 0){
    if(cameraCmd != KNIFE_CMD_FIND_LINE){
      camera_SendCommand(KNIFE_CMD_FIND_LINE);
    }
    save = gl_controlCenter.findLineWay;
    gl_controlCenter.findLineWay = FL_KNIFE;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 1;
  }
  if(flag == 1 && getIsFindNode() == 1){
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 0;
    return 1;
  }
  return 0;
}
/**

* 函数介绍：下平台方法（通用模板）
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他		：通过陀螺仪俯仰角检测位置
* 作者    ：

*/
static uint8_t BlockHandleMethod_Platform() {
  static FindLine save;
  static enum Flag{
    INIT,//初始化
    ADJUST,//校正
    ON_PLATFORM,//在平台上
    PRE_DOWN,//准备下坡,前轮刚到坡上
    DOWNING,//下坡中
  }flag;
  /*
    INIT->ADJUST->ON_PLATFORM->PRE_DOWN->DOWNING
             \                  /^
              \>>----------->>-/^
   */
  if(flag == INIT){
    LOG_INFO("下平台障碍方法,开始校正");
    save = gl_controlCenter.findLineWay;
    //发送进入校正
    camera_SendCommand(CAMERA_CMD_PLATFORM_ADJUST);
    gl_controlCenter.findLineWay = FL_high_adjust;
    gl_controlCenter.findLineFlag = 0;
    flag = ADJUST;
  }
  //接收到返回值，校正完成
  if(flag == ADJUST && getIsFindNode() == 1){
    LOG_INFO("校正完成,还在平台上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = ON_PLATFORM;
  }
  if(flag == ADJUST && CAR_ATTITUDE_Platform_down){
    LOG_INFO("没有校正完成,半个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN;
  }
  //开始下坡，改为慢速
  if(flag == ON_PLATFORM && CAR_ATTITUDE_Platform_down){//检测下平台
    LOG_INFO("整个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN;
  }
  if(flag == PRE_DOWN && CAR_ATTITUDE_Platform_down_ing){//检测下平台中
    LOG_INFO("下平台中");
    flag = DOWNING;
    gl_controlCenter.findLineWay = FL_DownPlatform;
    gl_controlCenter.findLineFlag = 0;
  }
  if(flag == DOWNING && CAR_ATTITUDE_Platform_down_finish){//检测下完平台
    LOG_INFO("已经在平地上");
    flag = INIT;
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  if(MyDistance((uint32_t)(BlockHandleMethod_Platform),500)){
    LOG_INFO("下平台不正常");
    flag = INIT;
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  return 0;
}
/**
 * @brief 下（一号平台）的处理方法
 * 
 * @return uint8_t 
 */
uint8_t BlockHandleMethod_PlatformOne(){
  static uint8_t flag = 0;
  static FindLine save;
  const int armUpTime = 500;
  if(flag == 0){
    LOG_INFO("下一号平台障碍方法,等待前挡板放下");
    save = gl_controlCenter.findLineWay;
    gl_controlCenter.findLineWay = FL_No;//把电机的控制权收回
    gl_controlCenter.findLineFlag = 0;
    motorStop();
    flag = 1;
  }
  if(flag == 1 && PES_FRONT == 0){//检测前挡板放下
    LOG_INFO("检测到前挡板放下");
    flag = 2;
  }
  if(flag == 2 && PES_FRONT == 1){//检测前挡板抬起
    LOG_INFO("检测到前挡板抬起,准备完毕,举手");
    flag = 3;
    playMusic(VOICE_Platform1_OK); //出发语音-“准备完毕”
    setServoAngleByIndex(SERVO_LEFT_ARM, SERVO_LEFT_ARM_UP);
    setServoAngleByIndex(SERVO_RIGHT_ARM, SERVO_RIGHT_ARM_UP);
  }
  if(flag == 3 && myDelay((uint32_t)BlockHandleMethod_PlatformOne,armUpTime)){
    LOG_INFO("举手完毕,放手,开始下平台");
    flag = 4;
    setServoAngleByIndex(SERVO_LEFT_ARM, SERVO_LEFT_ARM_INIT);
    setServoAngleByIndex(SERVO_RIGHT_ARM, SERVO_RIGHT_ARM_INIT);
  }
  if(flag == 4 && BlockHandleMethod_Platform()==1){//开始下平台，返回1就是下完平台
    LOG_INFO("下完平台,放下身体");
    flag = 0;
    setServoAngleByIndex(SERVO_BODY, SERVO_BODY_FRONT);
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  return 0;
}
/**
 * @brief 小山丘平台的处理方法
 * 
 * @return uint8_t 
 */
static uint8_t BlockHandleMethod_Hill() {
  #define HILL_DISTANCE 800
  static uint8_t flag = 0;
  if (flag == 0) {
    setServoAngleByIndex(SERVO_BODY,SERVO_BODY_FRONT);
    flag = 1;
  }
  if(flag == 1 && CAR_ATTITUDE_Hill_Peak_up){//检测上坡
    LOG_INFO("检测到上坡");
    gl_controlCenter.findLineWay = FL_UP_LONG;
    gl_controlCenter.findLineFlag = 0;
    flag = 2;
  }
  if(flag == 2 && MyDistance((uint32_t)BlockHandleMethod_Hill,HILL_DISTANCE)==true){//上坡的末端，已经看不到线了
    LOG_INFO("已经走了%dmm,认为已经看不到线了,切换为盲走上平台",HILL_DISTANCE);
    gl_controlCenter.findLineWay = NFL_GYRO_UP;
    gl_controlCenter.findLineFlag = 0;
    flag = 3;
  }
  if(flag == 3 && CAR_ATTITUDE_Hill_Peak_up_end){//检测上完坡
    LOG_INFO("检测到上完坡");
    flag = 0;
    return 1;
  }
  return 0;
}
static uint8_t BlockHandleMethod_HillDown(){
  static FindLine save;
  static enum Flag{
    INIT,//初始化
    BACK,//后退,为了更好的校正
    ADJUST,//校正
    ON_PLATFORM,//在平台上
    PRE_DOWN,//准备下坡,前轮刚到坡上
    DOWNING,//下坡中
  }flag;
  /*
    INIT->BACK->ADJUST->ON_PLATFORM->PRE_DOWN->DOWNING
                   \                  /^
                    \>>---------->>--/^
  */
  #define VALUE1 500 // FL_PreDown速度下，从前轮刚下坡，角度大于4度到整车完全在坡上的时间
  const int backTime = 125;
  if (flag == INIT) {
    LOG_INFO("下山丘平台障碍方法,开始后退");
    save = gl_controlCenter.findLineWay;
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(-1200, -1200);
    flag = BACK;
  }
  if (flag == BACK && myDelay((uint32_t)BlockHandleMethod_HillDown, backTime)) {
    LOG_INFO("后退时间:%d,完毕,开始校正",backTime);
    // 发送进入校正
    camera_SendCommand(CAMERA_CMD_PLATFORM_ADJUST);
    gl_controlCenter.findLineWay = FL_high_adjust;
    gl_controlCenter.findLineFlag = 0;
    flag = ADJUST;
  }
  if (flag == ADJUST && getIsFindNode() == 1) {
    LOG_INFO("校正完成,还在平台上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = ON_PLATFORM;
  }
  if (flag == ADJUST && CAR_ATTITUDE_HillDown_PeakDown_down){//检测下平台
    LOG_INFO("没有校正完成,半个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN;
  }
  //开始下坡，改为慢速
  if(flag == ON_PLATFORM && CAR_ATTITUDE_HillDown_PeakDown_down){//检测下平台
    LOG_INFO("半个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN;
  }
  //到达坡上，改为中速
  if(flag == PRE_DOWN && myDelay((uint32_t)BlockHandleMethod_HillDown,VALUE1)){//检测下坡中
  //使用时间判断,从半个车在坡上到整个车在坡上所花费的时间: VALUE1 ms,认为整个车已经在下坡上
    LOG_INFO("整个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_DOWN_LONG;
    gl_controlCenter.findLineFlag = 0;
    flag = DOWNING;
  }
  if(flag == DOWNING && CAR_ATTITUDE_Platform_down_finish){//检测下完平台
    LOG_INFO("已经在平地上");
    flag = INIT;
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  return 0;
#undef VALUE1
}
/**
 * @brief 上珠峰平台的处理方法
 * 
 * @return uint8_t 
 */
static uint8_t BlockHandleMethod_Peak(){
  #define PEAK_DISTANCE_1 700
  #define PEAK_DISTANCE_2 700
  static uint8_t flag = 0;
  static FindLine save;
  if(flag == 0){
    LOG_INFO("上珠峰平台障碍方法");
    save = gl_controlCenter.findLineWay;
    flag = 1;
  }
  if(flag == 1 && CAR_ATTITUDE_Hill_Peak_up){//检测上坡
    LOG_INFO("检测到上坡");
    gl_controlCenter.findLineWay = FL_UP_LONG;
    gl_controlCenter.findLineFlag = 0;
    flag = 2;
  }
  if(flag == 2 && MyDistance((uint32_t)BlockHandleMethod_Peak,PEAK_DISTANCE_1)==true){//上坡的末端，已经看不到线了
    LOG_INFO("已经走了%dmm,认为已经看不到线了,切换为盲走上平台",PEAK_DISTANCE_1);
    gl_controlCenter.findLineWay = NFL_GYRO_UP;
    gl_controlCenter.findLineFlag = 0;
    flag = 3;
  }
  if(flag == 3 && CAR_ATTITUDE_flat){//检测上完坡,已经在半山腰的平台上了
    LOG_INFO("检测到上完第一个坡,开始校正");
    //发送进入校正
    camera_SendCommand(CAMERA_CMD_PLATFORM_ADJUST);
    gl_controlCenter.findLineWay = FL_high_adjust;
    //gl_controlCenter.findLineWay = NFL_GYRO;
    gl_controlCenter.findLineFlag = 0;
    flag = 4;
  }
  if(flag == 4 && (CAR_ATTITUDE_Hill_Peak_up || getIsFindNode() == 1)){//山腰后的上坡
    LOG_INFO("校正完成,开始第二段上坡");
    gl_controlCenter.findLineWay = FL_UP_LONG_SLOW;
    gl_controlCenter.findLineFlag = 0;
    flag = 5;
  }
  if(flag == 5 && MyDistance((uint32_t)BlockHandleMethod_Peak,PEAK_DISTANCE_2)==true){//上坡的末端，已经看不到线了
    LOG_INFO("已经走了%dmm,认为已经看不到线了,切换为盲走上平台",PEAK_DISTANCE_2);
    gl_controlCenter.findLineWay = NFL_GYRO_UP;
    gl_controlCenter.findLineFlag = 0;
    flag = 6;
  }
  if(flag == 6 && CAR_ATTITUDE_Hill_Peak_up_end){//检测上完坡，到达世界最高峰——理塘
    LOG_INFO("检测到上完第二个坡");
    flag = 0;
    return 1;
  }
  return 0;
}
/**
 * @brief 下珠峰平台的处理方法
 * 
 * @return uint8_t 
 */
static uint8_t BlockHandleMethod_PeakDown(){
  static enum Flag{
    INIT,//初始化
    BACK,//后退,为了更好的校正
    ADJUST,//校正
    ON_PLATFORM,//在平台上
    PRE_DOWN,//准备下坡,前轮刚到坡上
    DOWN_LONG,//下坡
    DOWN_END,//下坡后段，快到山腰
    ON_LOW_PLATFORM,//低平台平地走
    ADJUST_2,//第二段校正
    ON_PLATFORM_2,//在平台上
    PRE_DOWN_2,//准备下坡,前轮刚到坡上
    DOWN_LONG_2,//下坡
  }flag;
  /*
    INIT->BACK->ADJUST->ON_PLATFORM->PRE_DOWN->DOWN_LONG->DOWN_END
                    \                  /^
                      \>>---------->>--/^

    DOWN_END->ADJUST_2->ON_PLATFORM_2->PRE_DOWN_2->DOWN_LONG_2
                    \                    /^
                      \>>---------->>----/^
  */
  static FindLine save;
  #define VALUE1 100 // FL_PreDown速度下，从前轮刚下坡，角度大于4度到整车完全在坡上的时间
  if(flag == INIT){
    LOG_INFO("下珠峰平台障碍方法,开始后退");
    save = gl_controlCenter.findLineWay;
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(-1200, -1200);
    flag = BACK;
  }
  if(flag == BACK && myDelay((uint32_t)BlockHandleMethod_PeakDown,125)){
    LOG_INFO("后退时间:%d,完毕,开始校正",125);
    motorStop();
    //发送进入校正
    camera_SendCommand(CAMERA_CMD_PLATFORM_ADJUST);
    gl_controlCenter.findLineWay = FL_high_adjust;
    gl_controlCenter.findLineFlag = 0;
    flag = ADJUST;
  }
  if(flag == ADJUST && getIsFindNode() == 1){
    LOG_INFO("校正完成,还在平台上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = ON_PLATFORM;
  }
  if(flag == ADJUST && CAR_ATTITUDE_HillDown_PeakDown_down){//检测下坡
    LOG_INFO("没有校正完成,半个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN;
  }
  //开始下坡，改为慢速
  if(flag == ON_PLATFORM && CAR_ATTITUDE_HillDown_PeakDown_down){//检测下坡
    LOG_INFO("半个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN;
  }
  if(flag == PRE_DOWN && myDelay((uint32_t)BlockHandleMethod_PeakDown,VALUE1)){//检测下坡中
    LOG_INFO("整个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_DOWN_LONG_SLOW;
    gl_controlCenter.findLineFlag = 0;
    flag = DOWN_LONG;
  }
  if(flag == DOWN_LONG && MyDistance((uint32_t)BlockHandleMethod_PeakDown,PEAK_DISTANCE_2+100)==true){//到达山腰
    LOG_INFO("已经走了%dmm,认为已经快到山腰了,没有线可以巡",PEAK_DISTANCE_2+100);
    gl_controlCenter.findLineWay = NFL_GYRO_DOWN;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = DOWN_END;
  }
  if(flag == DOWN_END && CAR_ATTITUDE_PeakDown_down_end)
  {
    gl_controlCenter.findLineWay = NFL_GYRO;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = ON_LOW_PLATFORM;
  }
  //到达山腰，开始校正第二段下坡
  if(flag == ON_LOW_PLATFORM && MyDistance((uint32_t)BlockHandleMethod_PeakDown,0)){
    //发送进入校正
    camera_SendCommand(CAMERA_CMD_PLATFORM_ADJUST);
    gl_controlCenter.findLineWay = FL_high_adjust;
    gl_controlCenter.findLineFlag = 0;
    flag = ADJUST_2;
  }
  //第二段校正完成
  if(flag == ADJUST_2 && getIsFindNode() == 1){
    LOG_INFO("校正完成,还在山腰上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = ON_PLATFORM_2;
  }
  if(flag == ADJUST_2 && CAR_ATTITUDE_HillDown_PeakDown_down){//山腰后的下坡
    LOG_INFO("没有校正完成,半个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN_2;
  }
  if(flag == ON_PLATFORM_2 && CAR_ATTITUDE_HillDown_PeakDown_down){//山腰后的下坡
    LOG_INFO("半个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_PreDown;
    gl_controlCenter.findLineFlag = 0;
    flag = PRE_DOWN_2;
  }
  if(flag == PRE_DOWN_2 && myDelay((uint32_t)BlockHandleMethod_PeakDown,VALUE1)){//检测下坡中
    LOG_INFO("整个车已经在下坡上");
    gl_controlCenter.findLineWay = FL_DOWN_LONG;
    gl_controlCenter.findLineFlag = 0;
    flag = DOWN_LONG_2;
  }
  if(flag == DOWN_LONG_2 && CAR_ATTITUDE_PeakDown_down_end){//检测下完平台
    LOG_INFO("已经在平地上");
    flag = INIT;
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    return 1;
  }
  return 0;
  #undef VALUE1
}
/**
 * @brief 跷跷板的处理办法
 * 
 * @return uint8_t 1(路障解决)  0(路障未解决)
 */
static uint8_t BlockHandleMethod_Seesaw(void)
{
  static uint8_t flag = 0;
  static FindLine save;
  if(flag == 0){
    save = gl_controlCenter.findLineWay;
    camera_SendCommand(CAMERA_CMD_SEESAW);
    gl_controlCenter.findLineWay = FL_seesaw;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 2;
  }
  //当跷跷板走到中间的时候改为慢速
  if(flag == 2 && (MyDistance((uint32_t)(BlockHandleMethod_Seesaw),450) == true))
  {
    gl_controlCenter.findLineWay = FL_seesaw_slow;
    gl_controlCenter.findLineFlag = 0;
    // motorStop();
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 3;
  }
  if(flag == 3 && (gyro_getPitch() - abs16((int)gyro_getRoll()) > -20))
  {
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(-1500,-1500);
    // motorStop();
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 4;
  }
  if(flag == 4 && (gyro_getPitch() > -5))
  {
    gl_controlCenter.findLineWay = FL_No;
    positionSpeedOut_2(2000,2000,2000,500);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 5;
  }
  //加速延时一段时间之后减速
  if(flag == 5 && myDelay((uint32_t)BlockHandleMethod_Seesaw,250) == true)
  {
    camera_SendCommand(CAMERA_CMD_FIND_LINE);
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(700,1600);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 6;
  }
  //当跷跷板基本到达平面的时候则完成过障碍
  if(flag == 6 && gyro_getPitch() < 4)
  {
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    LOG_INFO("即将检测摄像头数据,数据为%d",getLineRank());
    flag = 7;
  }
  if(flag == 7 && getLineRank() != 80 && inRange(getLineRank(),20,140))
  {
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 0;
    return 1;
  }
  else if(flag == 7 && getLineRank() == 80)
  {
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(600,2000);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
  }
  return 0;
}
/**
 * @brief 圆弧的处理办法(16-44,46-43) (把圆弧当成一个路障)
 * 
 * @return uint8_t 1(路障解决)  0(路障未解决)
 */
static uint8_t BlockHandleMethod_Arc_16_44() {
  static uint8_t flag = 0;
  static FindLine save;
  static float yaw_error;
  static uint8_t stableNum = 0;
  //在一开始记录偏转角，目前是盲走一段距离
  if(flag == 0){
    yaw_error = gyro_getYaw();
    save = gl_controlCenter.findLineWay;
    gl_controlCenter.findLineWay = FL_middle;
    gl_controlCenter.findLineFlag = 0;
    flag = 1;
  }
 //之后进行默认寻线
  if(flag == 1 && MyDistance((uint32_t)(BlockHandleMethod_Arc_16_44),350) == true)
  {
    gl_controlCenter.findLineWay = FL_No;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 2;
  }
  uint8_t rotate_trace_time(Direction dir);
  if(flag == 2 && rotate_trace_time(Left)){
    gl_controlCenter.findLineWay = FL_Arc;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 3;
  }
  //当横滚角小于-10或俯仰角小于-4的时候倒车一段时间
  if(flag == 3 && (gyro_getRoll() < -10 || gyro_getPitch() < -4))
  {
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(0,0);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 4;
  }
  //电机暂停并计算一定的角度
  if(flag == 4 && /*gyro_getRoll() > -2*/myDelay((uint32_t)(BlockHandleMethod_Arc_16_44),0) == true)
  {
    gl_controlCenter.findLineWay = FL_No;
    motorStop();
    yaw_error = errCalculate(gyro_getYaw(),calculateYawAngle(yaw_error,90),Left);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 5;
  }
  //旋转一定的角度
  if(flag == 5 && (rotAngleCommon(yaw_error,Left) == 1))
  {
    gl_controlCenter.findLineWay = FL_No;
    motorStop();
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 6;
  }
  if(flag == 6 && myDelay((uint32_t)(BlockHandleMethod_Arc_16_44),100) == true)
  {
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
    flag = 0;
    return 1;
  }
  return 0;
}
/**
 * @brief 黑上
 * 
 * @return uint8_t 
 */
uint8_t BlockHandleMethod_Black_up(){
  static uint8_t flag = 0;
  static FindLine save;
  if(flag == 0){
    save = gl_controlCenter.findLineWay;
    flag = 1;
  }
  if(flag == 1 && gyro_getPitch() <-4){//检测前挡板放下
    gl_controlCenter.findLineWay = FL_default;
    gl_controlCenter.findLineWay = 0;
    flag = 2;
  }
  if(flag == 2 && gyro_getPitch() > 4){//检测前挡板抬起
    flag = 3;
  }
  if(flag == 3 && inRange((int)gyro_getPitch(),-4,4)){
    gl_controlCenter.findLineWay = save;
    gl_controlCenter.findLineFlag = 0;
    flag = 0;
    return 1;
  }
  return 0;
}
static uint8_t BlockHandleMethod_Door() {
  changeRunMethodTable(&gl_controlCenter, mapInfo, runMethodTable[2]);
  LOG_INFO("changeRunMethodTable");
  return 1;
}

/// @brief 路障任务回调函数
/// @param control 控制台
/// @note
/// 在完成路障任务后调用，将路障任务状态设置为完成，将加速任务状态置为准备就绪
static void roadBlocksCallback(ControlCenterTypeDef *control) {
  control->runState->F_RoadBlockState = MY_EXIT;
  control->runState->speedUpState = MY_READY;
}

/// @brief 路障任务处理函数
/// @param control 控制台
/// @note 在路障任务状态为准备就绪时调用，根据路障任务的类型执行不同的路障任务
/// @note 路障处理完成后调用路障任务回调函数
void roadBlocksHandle_Task(ControlCenterTypeDef *control) {
  int result = 0;
  // 如果路障任务状态为准备就绪
  if (control->runState->F_RoadBlockState == MY_READY) {
    // 根据路障任务的类型执行不同的路障任务，sectionInfo是当前路段的信息的指针，指向地图
    switch (control->sectionInfo->roadBlockWay) {
      case FREE:
        result = 1;
        break;
      case Platform_DOWN:
        result = BlockHandleMethod_Platform();
        break;
      case BRIDGE:
        result = BlockHandleMethod_Bridge();
        break;
      case STEP:
        result = BlockHandleMethod_Step();
        break;
      case Wave:
        result = BlockHandleMethod_Wave();
        break;
      case Wave_time:
        result = BlockHandleMethod_Wave_time(1100);
        break;
      case Wave_TWICE:
        result = BlockHandleMethod_Wave_time(2200);
        break;
      case Platform_DOWN_ONE:
        result = BlockHandleMethod_PlatformOne();
        break;
      case PEAK:
        result = BlockHandleMethod_Peak();
        break;
      case PEAK_DOWN:
        result = BlockHandleMethod_PeakDown();
        break;
      case HILL:
        result = BlockHandleMethod_Hill();
        break;
      case HILL_DOWN:
        result = BlockHandleMethod_HillDown();
        break;
      case KNIFE:
        result = BlockHandleMethod_Knife();
        break;
      case SEESAW:
        result = BlockHandleMethod_Seesaw();
        break;
      case ARC_16_44:
        result = BlockHandleMethod_Arc_16_44();
        break;
      case BLACK_UP:
        result = BlockHandleMethod_Black_up();
        break;
      default:
        result = 1;
        break;
    }
    if (result == 1) {
      roadBlocksCallback(control);
    }
  }
}
